{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "7082cdb7-1a88-4286-8503-1f3168cd2f57",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "import sys,random"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ef06817-67d7-4090-9de3-a36564d5a1dc",
   "metadata": {},
   "outputs": [],
   "source": [
    "df=pd.read_excel('题目/附件1：数据集1-终稿.xlsx', engine='openpyxl',skiprows=[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 111,
   "id": "1f654551-9eb6-41a8-bb34-2556cf70784b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>编号</th>\n",
       "      <th>X坐标（单位: m）</th>\n",
       "      <th>Y坐标（单位:m）</th>\n",
       "      <th>Z坐标（单位: m）</th>\n",
       "      <th>校正点类型</th>\n",
       "      <th>第三问点标记</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>0</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>50000.000000</td>\n",
       "      <td>5000.000000</td>\n",
       "      <td>A 点</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>1</td>\n",
       "      <td>33070.825831</td>\n",
       "      <td>2789.480761</td>\n",
       "      <td>5163.524680</td>\n",
       "      <td>0</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>2</td>\n",
       "      <td>54832.887019</td>\n",
       "      <td>49179.219108</td>\n",
       "      <td>1448.303791</td>\n",
       "      <td>1</td>\n",
       "      <td>1</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>3</td>\n",
       "      <td>77991.545911</td>\n",
       "      <td>63982.175286</td>\n",
       "      <td>5945.823038</td>\n",
       "      <td>0</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>4</td>\n",
       "      <td>16937.179535</td>\n",
       "      <td>84714.341903</td>\n",
       "      <td>5360.293002</td>\n",
       "      <td>0</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>...</th>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>608</th>\n",
       "      <td>608</td>\n",
       "      <td>45789.201490</td>\n",
       "      <td>21191.207906</td>\n",
       "      <td>440.001872</td>\n",
       "      <td>1</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>609</th>\n",
       "      <td>609</td>\n",
       "      <td>94917.734859</td>\n",
       "      <td>82958.725321</td>\n",
       "      <td>6169.657707</td>\n",
       "      <td>0</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>610</th>\n",
       "      <td>610</td>\n",
       "      <td>14870.601682</td>\n",
       "      <td>95939.173362</td>\n",
       "      <td>8248.836185</td>\n",
       "      <td>0</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>611</th>\n",
       "      <td>611</td>\n",
       "      <td>93009.567446</td>\n",
       "      <td>4549.333260</td>\n",
       "      <td>7882.608297</td>\n",
       "      <td>1</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>612</th>\n",
       "      <td>612</td>\n",
       "      <td>100000.000000</td>\n",
       "      <td>59652.343380</td>\n",
       "      <td>5022.001164</td>\n",
       "      <td>B点</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "<p>613 rows × 6 columns</p>\n",
       "</div>"
      ],
      "text/plain": [
       "      编号     X坐标（单位: m）     Y坐标（单位:m）   Z坐标（单位: m） 校正点类型  第三问点标记\n",
       "0      0       0.000000  50000.000000  5000.000000   A 点       0\n",
       "1      1   33070.825831   2789.480761  5163.524680     0       0\n",
       "2      2   54832.887019  49179.219108  1448.303791     1       1\n",
       "3      3   77991.545911  63982.175286  5945.823038     0       0\n",
       "4      4   16937.179535  84714.341903  5360.293002     0       0\n",
       "..   ...            ...           ...          ...   ...     ...\n",
       "608  608   45789.201490  21191.207906   440.001872     1       0\n",
       "609  609   94917.734859  82958.725321  6169.657707     0       0\n",
       "610  610   14870.601682  95939.173362  8248.836185     0       0\n",
       "611  611   93009.567446   4549.333260  7882.608297     1       0\n",
       "612  612  100000.000000  59652.343380  5022.001164    B点       0\n",
       "\n",
       "[613 rows x 6 columns]"
      ]
     },
     "execution_count": 111,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 112,
   "id": "4891ee98-a15c-467e-97cd-72e2258f607d",
   "metadata": {},
   "outputs": [],
   "source": [
    "df=df.drop(df.columns[0],axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "274ffbd5-06f2-4fd2-abef-0b34857f482f",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 113,
   "id": "4f274361-14cd-4417-84d2-ee9276eec17f",
   "metadata": {},
   "outputs": [],
   "source": [
    "point_list= df.values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 114,
   "id": "baf04101-cd1b-471a-81b6-51f82dcb4d29",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[0.0, 50000.0, 5000.0, 'A 点', 0],\n",
       "       [33070.825830821, 2789.48076085272, 5163.5246804925, 0, 0],\n",
       "       [54832.8870194109, 49179.2191080384, 1448.30379093285, 1, 1],\n",
       "       ...,\n",
       "       [14870.6016822319, 95939.1733623643, 8248.83618468285, 0, 0],\n",
       "       [93009.5674461602, 4549.3332597056, 7882.60829650082, 1, 0],\n",
       "       [100000.0, 59652.3433795158, 5022.00116448164, 'B点', 0]],\n",
       "      dtype=object)"
      ]
     },
     "execution_count": 114,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "point_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 115,
   "id": "25dccbce-05ee-4ee3-8568-7c72d76851d0",
   "metadata": {},
   "outputs": [],
   "source": [
    "point_num = len(point_list)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 116,
   "id": "bf14d5ac-6677-4433-a0f7-4a42d609a268",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "613"
      ]
     },
     "execution_count": 116,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "point_num"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 117,
   "id": "5f6459f1-c6d7-49ed-b88d-55e50df2eb6f",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "\n",
    "def get_distance(start_index, end_index):\n",
    "    x1, y1, z1 = point_list[start_index][0:3]\n",
    "    x2, y2, z2 = point_list[end_index][0:3]\n",
    "    return ((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2) ** 0.5\n",
    "\n",
    "\n",
    "def judge_z(start_index, end_index=point_num - 1):\n",
    "    x1, y1, z1 = point_list[start_index][0:3]\n",
    "    x2, y2, z2 = point_list[end_index][0:3]\n",
    "    if abs(z1 - z2) > 5000:\n",
    "        return False\n",
    "    else:\n",
    "        return True\n",
    "\n",
    "def judge(start_index, end_index, horizontal_error, vertical_error):\n",
    "    end_point_type = point_list[end_index][3]\n",
    "    distance = get_distance(start_index, end_index)\n",
    "\n",
    "    delta_error = distance * sigma\n",
    "    end_point_horizontal_error = horizontal_error + delta_error\n",
    "    end_point_vertical_error = vertical_error + delta_error\n",
    "    if judge_z(start_index):\n",
    "        if end_index != point_num - 1:  # ????????\n",
    "            if end_point_type == 0:  # ??\n",
    "                if end_point_horizontal_error <= beta2 and end_point_vertical_error <= beta1:\n",
    "                    is_pass = True\n",
    "                else:\n",
    "                    is_pass = False\n",
    "            elif end_point_type == 1:  # ??\n",
    "                if end_point_horizontal_error <= alpha2 and end_point_vertical_error <= alpha1:\n",
    "                    is_pass = True\n",
    "                else:\n",
    "                    is_pass = False\n",
    "            else:\n",
    "                is_pass = False\n",
    "        else:  # ???????\n",
    "            if end_point_horizontal_error <= theta and end_point_vertical_error <= theta:\n",
    "                is_pass = True\n",
    "            else:\n",
    "                is_pass = False\n",
    "    else:\n",
    "        is_pass = False\n",
    "    after_end_point_horizontal_error = end_point_horizontal_error\n",
    "    after_end_point_vertical_error = end_point_vertical_error\n",
    "    if is_pass:\n",
    "        if end_point_type == 0:\n",
    "            after_end_point_horizontal_error = 0\n",
    "            after_end_point_vertical_error = end_point_vertical_error\n",
    "        elif end_point_type == 1:\n",
    "            after_end_point_horizontal_error = end_point_horizontal_error\n",
    "            after_end_point_vertical_error = 0\n",
    "    return is_pass, end_point_horizontal_error, end_point_vertical_error,after_end_point_horizontal_error, after_end_point_vertical_error\n",
    "\n",
    "\n",
    "def rank_distance(point_index_list):\n",
    "    ranked_list = sorted(point_index_list, key=lambda index_list: get_distance(index_list[0], point_num - 1))\n",
    "    return ranked_list\n",
    "\n",
    "\n",
    "def get_all_distance(index_list):\n",
    "    distance = 0\n",
    "    for i in range(len(index_list) - 1):\n",
    "        start_index = index_list[i]\n",
    "        end_index = index_list[i + 1]\n",
    "        distance = distance + get_distance(start_index, end_index)\n",
    "    return distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 118,
   "id": "1a1de84d-563c-4d41-aec7-91d2103b415f",
   "metadata": {},
   "outputs": [],
   "source": [
    "def find_path(start_index=0, horizontal_error=0, vertical_error=0):\n",
    "    global temp_all_distance\n",
    "    global order, temp_order\n",
    "    order.append(start_index)\n",
    "    candidate_list = []\n",
    "    for index in range(point_num):\n",
    "        if index != start_index:\n",
    "            is_pass, end_point_horizontal_error, end_point_vertical_error, after_end_point_horizontal_error, after_end_point_vertical_error = judge(start_index, index, horizontal_error, vertical_error)\n",
    "            if is_pass and get_distance(start_index, point_num - 1) > get_distance(index,\n",
    "                                                                       point_num - 1):\n",
    "                candidate_list.append(index)\n",
    "    if len(candidate_list) == 0:\n",
    "        order.pop()\n",
    "        return\n",
    "    candidate_list.sort(key=lambda index_list: get_distance(index_list, point_num - 1))\n",
    "# random.shuffle(candidate_list)\n",
    "    if len(candidate_list) >= 5:\n",
    "        candidate_list = candidate_list[0:5]\n",
    "    for candidate in candidate_list:\n",
    "        if candidate == point_num - 1:\n",
    "            order.append(candidate)\n",
    "            all_distance = get_all_distance(order)\n",
    "            if all_distance < temp_all_distance:\n",
    "                temp_all_distance = all_distance\n",
    "                temp_order = order.copy()\n",
    "                print('\\n' + 'small', order, all_distance)\n",
    "            order.pop()\n",
    "            break\n",
    "        if len(order) > 13:\n",
    "            break\n",
    "        is_pass, end_point_horizontal_error, end_point_vertical_error, after_end_point_horizontal_error, after_end_point_vertical_error = judge(start_index, candidate, horizontal_error, vertical_error)\n",
    "        find_path(candidate, after_end_point_horizontal_error, after_end_point_vertical_error)\n",
    "\n",
    "    order.pop()\n",
    "    return"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 119,
   "id": "d74bb77a-4537-4a7a-9947-6a88de125fa0",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "random.seed(1)\n",
    "sys.setrecursionlimit(100000)\n",
    "alpha1 = 25\n",
    "alpha2 = 15\n",
    "beta1 = 20\n",
    "beta2 = 25\n",
    "theta = 30\n",
    "sigma = 0.001"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 120,
   "id": "87a4bc6c-a4a9-47f6-9f9c-9b5316ef5677",
   "metadata": {},
   "outputs": [],
   "source": [
    "order = []\n",
    "temp_all_distance = 1000000\n",
    "temp_order = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "891cda01-8ab9-4617-9278-e74608763bae",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "small [0, 303, 199, 15, 418, 11, 450, 3, 397, 612] 112329.74603335514\n",
      "\n",
      "small [0, 303, 199, 15, 418, 11, 450, 286, 368, 612] 111362.31025478707\n",
      "\n",
      "small [0, 303, 199, 15, 418, 11, 450, 286, 397, 612] 111046.2654120447\n",
      "\n",
      "small [0, 303, 199, 15, 418, 11, 369, 214, 397, 612] 110668.61320892324\n",
      "\n",
      "small [0, 303, 199, 15, 418, 278, 369, 214, 397, 612] 107253.70811782636\n",
      "\n",
      "small [0, 303, 199, 15, 56, 278, 369, 214, 397, 612] 106879.27501719673\n",
      "\n",
      "small [0, 303, 199, 15, 148, 278, 369, 214, 397, 612] 105865.20026694027\n"
     ]
    }
   ],
   "source": [
    "find_path(0,0,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 92,
   "id": "250bfc82-09f5-494c-b4e2-18fbaec45b1d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[0, 455, 338, 275, 610]"
      ]
     },
     "execution_count": 92,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "temp_order"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca8e2cda-9523-4e82-833d-daf4847751a6",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a6a09e53-17aa-42d0-b9b0-a1dbf0c67d22",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a3a8dd01-2a1b-4d6f-8c6a-234223effa7f",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
